
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_gcs.c
  * @author     baiyang
  * @date       2021-11-10
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <stdint.h>
#include <stdbool.h>

#include <rthw.h>
#include <rtthread.h>
#include <rtdevice.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>
#include <mavproxy/mavproxy.h>
#include <mavproxy/mavproxy_monitor.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct {
    uint8_t sysid;          ///< ID of message sender system/aircraft
    uint8_t compid;         ///< ID of the message sender component

    struct rt_ringbuffer msg_rb;       // id(4 byte) + sysid(1 byte) + compid(1 byte) + msg
    uint8_t msg_rb_pool[1024];

    mavlink_message_t mavlink_msg;

    struct rt_mutex _mutex;
} fms_gcs_msg;

/*---------------------------------prototype----------------------------------*/
static void fms_rc_manual_override(RC_HandleTypeDef *c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed);
static void fms_handle_rc_channels_override(const mavlink_rc_channels_override_t *packet);
static void fms_handle_set_mode(const mavlink_set_mode_t* set_mode, mavlink_message_t *msg);
static void fms_push_msg(uint32_t msg_id, uint8_t sysid, uint8_t compid, uint8_t *msg_buf, uint32_t msg_len);
static uint32_t fms_get_msg_id();
static void fms_get_msg_date(uint8_t *msg_buf, uint32_t msg_len);
static void fms_send_command_ack_sysid(uint16_t command, uint8_t result, mavlink_message_t* msg);
static void fms_proc_command(mavlink_command_long_t* command, mavlink_message_t* msg);
static MAV_RESULT fms_handle_command_component_arm_disarm(const mavlink_command_long_t *packet);
static void fms_send_command_ack(uint16_t command, uint8_t result, mavlink_message_t* msg);
static MAV_RESULT fms_handle_command_do_set_mode(const mavlink_command_long_t *packet);
static MAV_RESULT fms_set_mode_common(const MAV_MODE _base_mode, const uint32_t _custom_mode);
static void fms_gcs_cb(void *parameter);
static bool fms_should_handle_command_long(uint16_t command);
/*----------------------------------variable----------------------------------*/
static fms_gcs_msg gcs_msg;

static const float magic_force_arm_value = 2989.0f;
static const float magic_force_disarm_value = 21196.0f;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_gcs_init()
{
    rt_ringbuffer_init(&gcs_msg.msg_rb, gcs_msg.msg_rb_pool, sizeof(gcs_msg.msg_rb_pool));

    rt_mutex_init(&gcs_msg._mutex, "fms_gcs", RT_IPC_FLAG_FIFO);

    itc_subscribe(ITC_ID(mavlink_msg), fms_gcs_cb);
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_gcs_update()
{
    uint32_t msg_id;
    uint16_t len = rt_ringbuffer_space_len(&gcs_msg.msg_rb);
    if (rt_ringbuffer_data_len(&gcs_msg.msg_rb) < sizeof(uint32_t)) {
        // 没有要处理的数据，直接返回
        return;
    }

    msg_id = fms_get_msg_id();

    switch (msg_id) {
        case MAVLINK_MSG_ID_COMMAND_LONG: {
            mavlink_command_long_t command;
            fms_get_msg_date((uint8_t *)&command, sizeof(mavlink_command_long_t));

            mavlink_msg_command_long_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &command);
            fms_proc_command(&command, &gcs_msg.mavlink_msg);
        } break;

        case MAVLINK_MSG_ID_MANUAL_CONTROL:
        {
            mavlink_manual_control_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_manual_control_t));

            mavlink_system_t sysid_this_mav = mavproxy_get_system();

            if (packet.target != sysid_this_mav.sysid) {
                break; // only accept control aimed at us
            }

            if (packet.z < 0) { // Copter doesn't do negative thrust
                break;
            }

            uint32_t tnow = time_millis();

            fms_rc_manual_override(fms.channel_roll, packet.y, 1000, 2000, tnow, false);
            fms_rc_manual_override(fms.channel_pitch, packet.x, 1000, 2000, tnow, true);
            fms_rc_manual_override(fms.channel_throttle, packet.z, 0, 1000, tnow, false);
            fms_rc_manual_override(fms.channel_yaw, packet.r, 1000, 2000, tnow, false);
            break;
        }
    
        case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
        {
            mavlink_rc_channels_override_t packet;
            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_rc_channels_override_t));

            fms_handle_rc_channels_override(&packet);
            break;
        }

        case MAVLINK_MSG_ID_SET_MODE:
        {
            mavlink_set_mode_t packet;

            fms_get_msg_date((uint8_t *)&packet, sizeof(mavlink_set_mode_t));
            mavlink_msg_set_mode_encode(gcs_msg.sysid, gcs_msg.compid, &gcs_msg.mavlink_msg, &packet);

            fms_handle_set_mode(&packet, &gcs_msg.mavlink_msg);
            break;
        }

        default: {
            // console_printf("unknown mavlink msg:%d\n", msg->msgid);
        } break;
    }
}

static void fms_rc_manual_override(RC_HandleTypeDef *c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed)
{
    if (c == NULL) {
        return;
    }
    int16_t override_value = 0;
    if (value_in != INT16_MAX) {
        const int16_t radio_min = c->_radio_min;
        const int16_t radio_max = c->_radio_max;
        if (reversed) {
            value_in *= -1;
        }
        override_value = radio_min + (radio_max - radio_min) * (value_in + offset) / scaler;
    }
    RC_set_override(c, override_value, tnow);
}

// allow override of RC channel values for complete GCS
// control of switch position and RC PWM values.
static void fms_handle_rc_channels_override(const mavlink_rc_channels_override_t *packet)
{
    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    if(packet->target_system != sysid_this_mav.sysid) {
        return; // Only accept control from our gcs
    }

    const uint32_t tnow = time_millis();

    const uint16_t override_data[] = {
        packet->chan1_raw,
        packet->chan2_raw,
        packet->chan3_raw,
        packet->chan4_raw,
        packet->chan5_raw,
        packet->chan6_raw,
        packet->chan7_raw,
        packet->chan8_raw,
        packet->chan9_raw,
        packet->chan10_raw,
        packet->chan11_raw,
        packet->chan12_raw,
        packet->chan13_raw,
        packet->chan14_raw,
        packet->chan15_raw,
        packet->chan16_raw
    };

    for (uint8_t i=0; i<8; i++) {
        // Per MAVLink spec a value of UINT16_MAX means to ignore this field.
        if (override_data[i] != UINT16_MAX) {
            RC_set_override(RCs_get_channel(i), override_data[i], tnow);
        }
    }
    for (uint8_t i=8; i<ARRAY_SIZE(override_data); i++) {
        // Per MAVLink spec a value of zero or UINT16_MAX means to
        // ignore this field.
        if (override_data[i] != 0 && override_data[i] != UINT16_MAX) {
            // per the mavlink spec, a value of UINT16_MAX-1 means
            // return the field to RC radio values:
            const uint16_t value = override_data[i] == (UINT16_MAX-1) ? 0 : override_data[i];
            RC_set_override(RCs_get_channel(i), value, tnow);
        }
    }

    //gcs().sysid_myggcs_seen(tnow);

}

/**
  * @brief       
  * @param[in]   set_mode  
  * @param[in]   msg  
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_handle_set_mode(const mavlink_set_mode_t* set_mode, mavlink_message_t *msg)
{
    const MAV_MODE _base_mode = (MAV_MODE)set_mode->base_mode;
    const uint32_t _custom_mode = set_mode->custom_mode;

    const MAV_RESULT result = fms_set_mode_common(_base_mode, _custom_mode);

    // send ACK or NAK.  Note that this is extraodinarily improper -
    // we are sending a command-ack for a message which is not a
    // command.  The command we are acking (ID=11) doesn't actually
    // exist, but if it did we'd probably be acking something
    // completely unrelated to setting modes.
    fms_send_command_ack_sysid(MAVLINK_MSG_ID_SET_MODE, result, msg);
}

static void fms_send_command_ack_sysid(uint16_t command, uint8_t result,
    mavlink_message_t* msg)
{
    mavlink_command_ack_t command_ack = {command, result, 0, 0, 0, 0};

    mavlink_msg_command_ack_encode(msg->sysid, msg->compid,
        msg, &command_ack);

    mavproxy_send_immediate_msg(msg, 1);
}

static void fms_proc_command(mavlink_command_long_t* command, mavlink_message_t* msg)
{
    MAV_RESULT result = MAV_RESULT_FAILED;

    switch (command->command) {
    case MAV_CMD_COMPONENT_ARM_DISARM:
        result = fms_handle_command_component_arm_disarm(command);
        break;

    case MAV_CMD_DO_SET_MODE:
        result = fms_handle_command_do_set_mode(command);
        break;

    default:
        break;
    }

    fms_send_command_ack(command->command, result, msg);
}

static void fms_send_command_ack(uint16_t command, uint8_t result, mavlink_message_t* msg)
{
    mavlink_command_ack_t command_ack = {command, result, 0, 0, 0, 0};
    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    mavlink_msg_command_ack_encode(sysid_this_mav.sysid, sysid_this_mav.compid,
        msg, &command_ack);

    mavproxy_send_immediate_msg(msg, 1);
}

static MAV_RESULT fms_handle_command_component_arm_disarm(const mavlink_command_long_t *packet)
{
    if (math_flt_equal(packet->param1,1.0f)) {
        if (arming_is_armed(&fms.arming)) {
            return MAV_RESULT_ACCEPTED;
        }
        // run pre_arm_checks and arm_checks and display failures
        const bool do_arming_checks = !math_flt_equal(packet->param2,magic_force_arm_value);
        if (arming_arm(&fms.arming, ARMING_CHECK_MAVLINK, do_arming_checks)) {
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
    }
    if (math_flt_zero(packet->param1))  {
        if (!arming_is_armed(&fms.arming)) {
            return MAV_RESULT_ACCEPTED;
        }
        const bool forced = math_flt_equal(packet->param2, magic_force_disarm_value);
        // note disarm()'s second parameter is "do_disarm_checks"
        if (arming_disarm(&fms.arming, ARMING_CHECK_MAVLINK, !forced)) {
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_FAILED;
    }

    return MAV_RESULT_UNSUPPORTED;
}

static MAV_RESULT fms_handle_command_do_set_mode(const mavlink_command_long_t *packet)
{
    const MAV_MODE _base_mode = (MAV_MODE)packet->param1;
    const uint32_t _custom_mode = (uint32_t)packet->param2;

    return fms_set_mode_common(_base_mode, _custom_mode);
}

/*
  code common to both SET_MODE mavlink message and command long set_mode msg
*/
static MAV_RESULT fms_set_mode_common(const MAV_MODE _base_mode, const uint32_t _custom_mode)
{
    // only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes
    if ((uint32_t)_base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
        if (!fms_set_mode(_custom_mode, MODE_REASON_GCS_COMMAND)) {
            // often we should be returning DENIED rather than FAILED
            // here.  Perhaps a "has_mode" callback on AP_::vehicle()
            // would do?
            return MAV_RESULT_FAILED;
        }
        return MAV_RESULT_ACCEPTED;
    }

    if (_base_mode == (MAV_MODE)MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
        // set the safety switch position. Must be in a command by itself
        if (_custom_mode == 0) {
            // turn safety off (pwm outputs flow to the motors)
            //hal.rcout->force_safety_off();
            return MAV_RESULT_ACCEPTED;
        }
        if (_custom_mode == 1) {
#if 0
            // turn safety on (no pwm outputs to the motors)
            if (hal.rcout->force_safety_on()) {
                return MAV_RESULT_ACCEPTED;
            }
            return MAV_RESULT_FAILED;
#endif
            return MAV_RESULT_ACCEPTED;
        }
        return MAV_RESULT_DENIED;
    }

    // Command is invalid (is supported but has invalid parameters)
    return MAV_RESULT_DENIED;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_gcs_cb(void *parameter)
{
    uitc_mavlink_msg *mavlink_msg = (uitc_mavlink_msg *)parameter;
    uint32_t msg_id;
    uint8_t *msg_buf;
    uint32_t msg_len;

    switch (mavlink_msg->msg_t->msgid) {
        case MAVLINK_MSG_ID_COMMAND_LONG: {
            mavlink_command_long_t command;
            mavlink_msg_command_long_decode(mavlink_msg->msg_t, &command);

            if (fms_should_handle_command_long(command.command)) {
                fms_push_msg(MAVLINK_MSG_ID_COMMAND_LONG, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&command, sizeof(mavlink_command_long_t));
            }
        } break;

        case MAVLINK_MSG_ID_MANUAL_CONTROL:
        {
            mavlink_manual_control_t packet;
            mavlink_msg_manual_control_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_MANUAL_CONTROL, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_manual_control_t));
            break;
        }
    
        case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
        {
            mavlink_rc_channels_override_t packet;
            mavlink_msg_rc_channels_override_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_rc_channels_override_t));
            break;
        }

        case MAVLINK_MSG_ID_SET_MODE:
        {
            mavlink_set_mode_t packet;
            mavlink_msg_set_mode_decode(mavlink_msg->msg_t, &packet);

            fms_push_msg(MAVLINK_MSG_ID_SET_MODE, mavlink_msg->msg_t->sysid, mavlink_msg->msg_t->compid, (uint8_t *)&packet, sizeof(mavlink_set_mode_t));
            break;
        }

        default: {
            // console_printf("unknown mavlink msg:%d\n", msg->msgid);
        } break;
    }
}

/**
  * @brief       
  * @param[in]   msg_id  
  * @param[in]   msg_buf  
  * @param[in]   msg_len  
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_push_msg(uint32_t msg_id, uint8_t sysid, uint8_t compid, uint8_t *msg_buf, uint32_t msg_len)
{
    if (rt_ringbuffer_space_len(&gcs_msg.msg_rb) >= msg_len + sizeof(msg_id) + sizeof(sysid) + sizeof(compid)) {

        rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);

        rt_ringbuffer_put(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, sysid);
        rt_ringbuffer_putchar(&gcs_msg.msg_rb, compid);
        rt_ringbuffer_put(&gcs_msg.msg_rb, msg_buf, msg_len);

        rt_mutex_release(&gcs_msg._mutex);
    } else {
        mavproxy_send_statustext(MAV_SEVERITY_INFO, "fms_gcs miss msg");
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static uint32_t fms_get_msg_id()
{
    uint32_t msg_id;
    rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
    rt_ringbuffer_get(&gcs_msg.msg_rb, (uint8_t *)&msg_id, sizeof(msg_id));
    rt_mutex_release(&gcs_msg._mutex);

    return msg_id;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void fms_get_msg_date(uint8_t *msg_buf, uint32_t msg_len)
{
    rt_mutex_take(&gcs_msg._mutex, RT_WAITING_FOREVER);
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.sysid);
    rt_ringbuffer_getchar(&gcs_msg.msg_rb, &gcs_msg.compid);
    rt_ringbuffer_get(&gcs_msg.msg_rb, msg_buf, msg_len);
    rt_mutex_release(&gcs_msg._mutex);
}

static bool fms_should_handle_command_long(uint16_t command)
{
    bool res = false;

    switch (command) {
    case MAV_CMD_COMPONENT_ARM_DISARM:
    case MAV_CMD_DO_SET_MODE:
        res = true;
        break;
    default:
        break;
    }

    return res;
}

/*------------------------------------test------------------------------------*/


